/**
 * @Brief: active emergency brake      
 * @Details: 主动紧急制动模块，根据底盘反馈主动避障，不受定位及导航影响
 * @Author: caylaxu
 * @Date: 2023-05-06 11:16
 */
#include "aeb/aeb.h"
#include "aeb/node.h"

#include <xmlrpc-c/base.hpp>
#include <xmlrpc-c/registry.hpp>
#include <xmlrpc-c/server.h>

rclcpp::Node::SharedPtr node;

AEBNode::AEBNode()
{
  node->declare_parameter<bool>("enable_aeb");
  bool_enable_aeb = node->get_parameter("enable_aeb").as_bool();
  node->declare_parameter<std::string>("cmd_vel_topic", "/cmd_vel");
  cmd_vel_topic = node->get_parameter("cmd_vel_topic").as_string();
  node->declare_parameter<std::string>("pub_cmd_vel_topic", "/aeb_cmd_vel");
  pub_cmd_vel_topic = node->get_parameter("pub_cmd_vel_topic").as_string();
  node->declare_parameter<std::string>("feedback_cmd_vel_topic", "/odom");
  feedback_cmd_vel_topic = node->get_parameter("feedback_cmd_vel_topic").as_string();
  node->declare_parameter<std::string>("obstacles_topic", "/obstacle_cloud");
  obstacles_topic = node->get_parameter("obstacles_topic").as_string();
  node->declare_parameter<std::string>("global_frame", "map");
  global_frame_ = node->get_parameter("global_frame").as_string();
  node->declare_parameter<std::string>("base_link_frame", "base_link");
  base_link_frame_ = node->get_parameter("base_link_frame").as_string();
  node->declare_parameter<double>("feedback_valid_speed_limit", 0.001);
  feedback_valid_speed_limit = node->get_parameter("feedback_valid_speed_limit").as_double();
  node->declare_parameter<double>("feedback_valid_angular_limit", 0.05);
  feedback_valid_angular_limit = node->get_parameter("feedback_valid_angular_limit").as_double();
  node->declare_parameter<double>("feedback_valid_time_limit", 0.5);
  feedback_valid_time_limit = node->get_parameter("feedback_valid_time_limit").as_double();
  node->declare_parameter<double>("straight_angle_limit", 0.001);
  straight_angle_limit = node->get_parameter("straight_angle_limit").as_double();
  node->declare_parameter<double>("deceleration", 0.2);
  deceleration_ = node->get_parameter("deceleration").as_double();
  node->declare_parameter<double>("acceleration", 0.2);
  acceleration_ = node->get_parameter("acceleration").as_double();
  node->declare_parameter<double>("angle_deceleration", 0.1);
  angle_deceleration_ = node->get_parameter("angle_deceleration").as_double();
  node->declare_parameter<double>("expansion_range", 0.2);
  expansion_range_ = node->get_parameter("expansion_range").as_double();
  node->declare_parameter<double>("dis_spacing", 0.3);
  dis_spacing_ = node->get_parameter("dis_spacing").as_double();
  node->declare_parameter<double>("angle_spacing", 0.26);
  angle_spacing_ = node->get_parameter("angle_spacing").as_double();
  node->declare_parameter<double>("holding_time", 5.0);
  holding_time_ = node->get_parameter("holding_time").as_double();
  node->declare_parameter<double>("aeb_obstacle_angular_speed", 0.1);
  aeb_obstacle_angular_speed_ = node->get_parameter("aeb_obstacle_angular_speed").as_double();
  node->declare_parameter<bool>("enable_auto_start_aeb");
  enable_auto_start_aeb_ = node->get_parameter("enable_auto_start_aeb").as_bool();

  node->declare_parameter<int>("stop_detecetion_threshold", 4);
  stop_detecetion_threshold_ = node->get_parameter("stop_detecetion_threshold").as_int();
  node->declare_parameter<int>("deceleration_detecetion_threshold", 3);
  deceleration_detecetion_threshold_ = node->get_parameter("deceleration_detecetion_threshold").as_int();

  node->declare_parameter<bool>("enable_smooth");
  enable_smooth_ = node->get_parameter("enable_smooth").as_bool();
  node->declare_parameter<double>("stop_distance", 0.2);
  stop_distance_ = node->get_parameter("stop_distance").as_double();
  node->declare_parameter<double>("stop_angular", 0.5);
  stop_angular_ = node->get_parameter("stop_angular").as_double();
  node->declare_parameter<double>("delay_time", 0.0);
  delay_time_ = node->get_parameter("delay_time").as_double();

  get_footprint_from_param();
  get_emergency_stop_footprint_from_param();
  if(!get_speed_limit_param()){
    rclcpp::shutdown();
  }
  tf_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
  tfl_ = std::make_shared<tf2_ros::TransformListener>(*tf_);

  last_control_time = rclcpp::Clock().now();
  last_smooth_time = rclcpp::Clock().now();

  // pose_pub = node->create_publisher<geometry_msgs::msg::PoseArray>("/reflector_array", 1);
  predict_path_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("/predict_path", 10);
  predict_stop_range_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("/predict_stop_range", 10);
  predict_deceleration_range_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>("/predict_deceleration_range", 10);
  emergency_stop_pub_ = node->create_publisher<std_msgs::msg::Bool>("/aeb/emergency_stop", 10);
  cmd_vel_pub_ = node->create_publisher<geometry_msgs::msg::Twist>(pub_cmd_vel_topic, 10);
  obstacle_range_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("/aeb/obstacle_range", 10);
  obstacles_pub_ = node->create_publisher<visualization_msgs::msg::Marker>("/aeb/obstacles", 10);
  status_pub_ = node->create_publisher<std_msgs::msg::String>("/avoidObs/robot_status", 10);
  obs_status_pub_ = node->create_publisher<std_msgs::msg::Int8>("/lidar_obstacle_status", 10); //兼容uqi状态机
  params_status_pub_ = node->create_publisher<ubt_interface::msg::Status>("/aeb/status", 10);

  obstacles_sub_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(obstacles_topic, 1, std::bind(&AEBNode::obstacles_callback, this, std::placeholders::_1));
  cmd_vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(cmd_vel_topic, 1, std::bind(&AEBNode::cmd_vel_callback, this, std::placeholders::_1));
  feedback_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(feedback_cmd_vel_topic, 1, std::bind(&AEBNode::feedback_callback, this, std::placeholders::_1));
  detection_timer  = node->create_wall_timer(std::chrono::milliseconds(int((0.05)*1000)),std::bind(&AEBNode::detection_timer_callback, this));

  enable_aeb_server = node->create_service<ubt_interface::srv::SetEnable>("/aeb/enable", std::bind(&AEBNode::enable_aeb, this, std::placeholders::_1, std::placeholders::_2));
  enable_aeb_forever_server = node->create_service<std_srvs::srv::SetBool>("/aeb/enable_forever", std::bind(&AEBNode::enable_aeb_forever, this, std::placeholders::_1, std::placeholders::_2));
  speed_limit_server = node->create_service<ubt_interface::srv::SetSpeedLimit>("/aeb/set_speed_limit", std::bind(&AEBNode::set_speed_limit, this, std::placeholders::_1, std::placeholders::_2));
  footprint_server = node->create_service<ubt_interface::srv::SetFootprint>("/aeb/set_footprint", std::bind(&AEBNode::set_footprint, this, std::placeholders::_1, std::placeholders::_2));
  stop_distance_server = node->create_service<ubt_interface::srv::SetStopDistance>("/aeb/set_stop_distance", std::bind(&AEBNode::set_stop_distance, this, std::placeholders::_1, std::placeholders::_2));
  query_server = node->create_service<ubt_interface::srv::QueryAebStatus>("/aeb/get_emergency_status", std::bind(&AEBNode::query_aeb_status, this, std::placeholders::_1, std::placeholders::_2));
}

bool AEBNode::enable_aeb(ubt_interface::srv::SetEnable::Request::ConstSharedPtr req, ubt_interface::srv::SetEnable::Response::SharedPtr res) 
{
  RCLCPP_INFO(node->get_logger(),"[AEB] set enable aeb %s", req->enable ? "true":"false");
  res->success = true;
  res->message = "set bool_enable_aeb success";
  if (req->enable)
  {
    node->declare_parameter<bool>("enable_auto_start_aeb");
    enable_auto_start_aeb_ = node->get_parameter("enable_auto_start_aeb").as_bool();
  }else{
    start_disable_aeb_time = rclcpp::Clock().now();
    if(req->disenable_time == 0.0)
    {
      node->declare_parameter<double>("disable_aeb_keeping_time", 15.0);
      disable_aeb_keeping_time_ = node->get_parameter("disable_aeb_keeping_time").as_double();
    }
    else
      disable_aeb_keeping_time_ = req->disenable_time;
  }  
  bool_enable_aeb = req->enable;
  return true;
}

bool AEBNode::set_speed_limit(ubt_interface::srv::SetSpeedLimit::Request::ConstSharedPtr req, ubt_interface::srv::SetSpeedLimit::Response::SharedPtr res)
{
  RCLCPP_INFO(node->get_logger(),"[AEB] set_speed_limit speed_limit %.3f angular_limit %.3f", req->speed_limit , req->angular_limit);
  static std::map<std::string, ubt_interface::srv::SetSpeedLimit::Request> speed_limit_list;

  if (req->speed_limit < 0 || req->angular_limit < 0)
  {
    res->success = false;
    res->message = "The limit speed is not allowed to be less than zero";
    return true;
  }

  speed_limit_list[req->source] = *req;

  speed_limit.speed_limit   = 999.0;
  speed_limit.angular_limit = 999.0;
  for(auto it : speed_limit_list) {
    speed_limit.speed_limit   = std::min(double(it.second.speed_limit),   speed_limit.speed_limit);
    speed_limit.angular_limit = std::min(double(it.second.angular_limit), speed_limit.angular_limit);
  }

  if(speed_limit.speed_limit == 0.0 || speed_limit.angular_limit == 0.0) {
    speed_limit.speed_limit = speed_limit.angular_limit = 0.0;
  }

  io_obstacle_ = speed_limit_list.count("io_obstacle") != 0 && speed_limit_list["io_obstacle"].speed_limit == 0.0;

  res->success = true;
  res->message = "set limit true";
  return true;
}

bool AEBNode::set_footprint(ubt_interface::srv::SetFootprint::Request::ConstSharedPtr req, ubt_interface::srv::SetFootprint::Response::SharedPtr res) 
{
  if(req->footprint.points.size() == 0) {
    footprint = first_footprint;
    res->success = true;
    RCLCPP_INFO(node->get_logger(),"[AEB] set_footprint [[%.3f,%.3f][%.3f,%.3f][%.3f,%.3f],[%.3f,%.3f]]", footprint[0].x, footprint[0].y, footprint[1].x, footprint[1].y,footprint[2].x, footprint[2].y,footprint[3].x, footprint[3].y);
    return true;
  }

  if(req->footprint.points.size() != 4) {
    res->success = false;
    res->message = "footprint size != 4";
    RCLCPP_WARN(node->get_logger(),"[AEB] set_footprint footprint size != 4");
    return true;
  }

  pcl::PointXY pt;
  footprint.clear();

  for(const auto& p : req->footprint.points) {
    pt.x = p.x;
    pt.y = p.y;
    footprint.push_back(pt);
  }
  RCLCPP_INFO(node->get_logger(),"[AEB] set_footprint [[%.3f,%.3f][%.3f,%.3f][%.3f,%.3f],[%.3f,%.3f]]", footprint[0].x, footprint[0].y, footprint[1].x, footprint[1].y,footprint[2].x, footprint[2].y,footprint[3].x, footprint[3].y);
  
  res->success = true;
  res->message = "success";
  return true;
}

bool AEBNode::set_stop_distance(ubt_interface::srv::SetStopDistance::Request::ConstSharedPtr req, ubt_interface::srv::SetStopDistance::Response::SharedPtr res)
{
  if(req->distance < 0.05 ) {
    node->declare_parameter<double>("stop_distance", 0.2);
    stop_distance_ = node->get_parameter("stop_distance").as_double();
    res->success = true;
    res->message = "set stop_distance to " + std::to_string(stop_distance_);
    RCLCPP_WARN(node->get_logger(),"[AEB] set stop_distance to %.3f", stop_distance_);
    return true;
  }

  stop_distance_ = req->distance;
  res->success = true;
  res->message = "set stop_distance to " + std::to_string(stop_distance_);
  RCLCPP_INFO(node->get_logger(),"[AEB] set stop_distance to %.3f", stop_distance_);
  return true;
}

bool AEBNode::enable_aeb_forever(std_srvs::srv::SetBool::Request::ConstSharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) 
{
  RCLCPP_INFO(node->get_logger(),"[AEB] set enable_aeb_forever %s", req->data ? "true":"false");
  if (req->data) //恢复避障,恢复自动恢复
  {
    node->declare_parameter<bool>("enable_auto_start_aeb");
    enable_auto_start_aeb_ = node->get_parameter("enable_auto_start_aeb").as_bool();
  }else{
    enable_auto_start_aeb_ = false;
  }
  bool_enable_aeb = req->data;
  res->success = true;
  res->message = "set enable_aeb_forever success";
  return true;
}

bool AEBNode::query_aeb_status(ubt_interface::srv::QueryAebStatus::Request::ConstSharedPtr req, ubt_interface::srv::QueryAebStatus::Response::SharedPtr res)
{
  RCLCPP_INFO(node->get_logger(),"[AEB] query_aeb !");
  res->header.stamp = holding_status.start_time;
  res->emergency_type = holding_status.type;
  res->emergency_type_str = aeb_type_str[holding_status.type];
  res->control_org = org_control;
  res->control_aeb = holding_status.twist;
  res->limit_speed = holding_status.linear_speed;
  res->obstacle_distance = holding_status.obstacle_distance;
  return true;
}

void AEBNode::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr twist)
{
  geometry_msgs::msg::Twist twist_after_limit;
  org_control = cal_twist_by_limits(*twist, speed_limit); //限速

  last_control_time = rclcpp::Clock().now();

  if (!bool_enable_aeb)
  {
    if (enable_auto_start_aeb_ && (rclcpp::Clock().now() - start_disable_aeb_time).seconds() > disable_aeb_keeping_time_)
    {
      RCLCPP_INFO(node->get_logger(),"[AEB] auto start aeb !");
      bool_enable_aeb = true;
    }else{
      //关闭aeb直接返回,避障急停需要自己解
      // cmd_vel_pub_->publish(org_control);
      publish_status(aeb_type::OTHER);
      // return;
    }
  }
  

  if(bool_enable_aeb)
  {
    // 触发io避障
    if(speed_limit.speed_limit == 0.0 && io_obstacle_) {
      obstacle_range.exist_obstacle = true;
      obstacle_range.type = EMERGENCY_STOP;
    }

    // RCLCPP_INFO(node->get_logger(),"exist_obstacle %d", obstacle_range.exist_obstacle);
    if (obstacle_range.exist_obstacle)
    {
      // RCLCPP_WARN(node->get_logger(),"type %d", obstacle_range.type);
      //1. 避停改为0速
      if (obstacle_range.type == EMERGENCY_STOP)
      {
        twist->linear.x = 0;
        twist->linear.y = 0;
        twist->angular.z = 0;
        set_holding_status(true, aeb_type::EMERGENCY_STOP, *twist, 0.0, obstacle_range.distance);
        publish_status(aeb_type::EMERGENCY_STOP);
      }
      else if (obstacle_range.type == SPEED_STOP)
      {
        twist->linear.x = 0;
        twist->linear.y = 0;
        twist->angular.z = 0;
        bool need_holding = check_need_holding(0);
        // std::cout << need_holding << " 1 " << holding_status.twist->linear.x << std::endl;
        if (!need_holding)
        {
          set_holding_status(true, aeb_type::SPEED_STOP, *twist, 0.0, obstacle_range.distance);
          publish_status(aeb_type::SPEED_STOP);
        }
      }
      else if (obstacle_range.type == DECELERATION) //2. 减速取限速和当前速度的最小值
      {
        double limit_speed = get_speed_limit(obstacle_range);
        bool need_holding = check_need_holding(limit_speed);
        // std::cout << need_holding << " 2 " << holding_status.twist->linear.x << std::endl;
        if (!need_holding)
        {
          set_holding_status(true, aeb_type::DECELERATION, *twist ,limit_speed, obstacle_range.distance);
          publish_status(aeb_type::DECELERATION);
        }else{
          limit_speed =  holding_status.linear_speed;
        }
        *twist = cal_twist(*twist, limit_speed, need_holding);
        holding_status.twist = *twist;
      }
    }else{
      bool need_holding = check_need_holding();
      // RCLCPP_INFO(node->get_logger(),"need_holding %d twist(%.3f, %.3f) holding_status.linear_speed %.3f", need_holding, twist->linear.x, twist->linear.y, holding_status.linear_speed);
      if (need_holding)
      {
        *twist = cal_twist(*twist, holding_status.linear_speed, need_holding);
      }else{
        set_holding_status(false, aeb_type::OTHER, *twist);
        publish_status(aeb_type::OTHER);
      }
    }
  }
  
  //3. 解除避障或者恢复速度保持之前的避障速度持续n s
  //其他啥也不干直接转发
  twist_after_limit = cal_twist_by_limits(*twist, speed_limit);
  // RCLCPP_INFO(node->get_logger(),"limits x z(%.3f, %.3f) ", twist_after_limit.linear.x, twist_after_limit.angular.z);
  //speed smooth
  if (enable_smooth_)
  {
    twist_after_limit = speed_smooth(twist_after_limit);
  }
  // RCLCPP_INFO(node->get_logger(),"mooth x z(%.3f, %.3f) ", twist_after_limit.linear.x, twist_after_limit.angular.z);

  cmd_vel_pub_->publish(twist_after_limit);
}

void AEBNode::obstacles_callback(const sensor_msgs::msg::PointCloud2::SharedPtr points_msg)
{
  std::lock_guard<std::mutex> lock(points_msg_now_mutex);
  sensor_msgs::msg::PointCloud2 cloud_out;
  if (points_msg->header.frame_id != base_link_frame_)
  {
    try{
      geometry_msgs::msg::TransformStamped trans = tf_->lookupTransform(base_link_frame_, points_msg->header.frame_id,rclcpp::Time(0));
      tf2::doTransform(*points_msg, cloud_out, trans);
    } catch (tf2::TransformException &ex) {
      RCLCPP_WARN(node->get_logger(),"%s", ex.what());
      return;
    }
  }else{
    cloud_out = *points_msg;
  }
  pcl::fromROSMsg(cloud_out, points_msg_now);
}

/**
 * @description: 碰撞检测
 * @param:  
 * @return:   
 */
void AEBNode::detection_timer_callback()
{
  publish_params_status();

  if ((rclcpp::Clock().now() - last_control_time).seconds() > feedback_valid_time_limit)
  {
    geometry_msgs::msg::Twist::SharedPtr twist_msg;
    cmd_vel_pub_->publish(*twist_msg);
    cmd_vel_callback(twist_msg);
    // last_control_time = rclcpp::Clock().now();
  }

  if (!bool_enable_aeb)
  {
    obstacle_range.exist_obstacle = false;
    holding_status.is_holding = false;
    holding_status.type = aeb_type::OTHER;
    emergency_detecetion_number = 0;
    stop_detecetion_number = 0;
    deceleration_detecetion_number = 0;
    stop_range_list.clear();
    deceleration_range_list.clear();
    publish_status(aeb_type::OTHER);
    return;
  }
  
  std::lock_guard<std::mutex> points_msg_now_lock(points_msg_now_mutex);
  std::lock_guard<std::mutex> stop_range_list_lock(stop_range_list_mutex);
  // 1. 先检查急停范围
  bool check_result = check_emergency_stop();

  if (check_result) {
    if(emergency_detecetion_number < stop_detecetion_threshold_) {
      emergency_detecetion_number++;
    } else {
      obstacle_range.exist_obstacle = true;
      obstacle_range.type = EMERGENCY_STOP;
      obstacle_range.range_index = 0;
      obstacle_range.distance = 0;
      return;
    }
  }else{
    emergency_detecetion_number = 0;
  }

  // 2. 遍历检测框，是否与点云有交集，有就停车
  std::vector<geometry_msgs::msg::Point > RectPoints;
  geometry_msgs::msg::Point obstacle_point;
  bool is_obstacle = false;
  for (size_t i = 0; i < stop_range_list.size(); i++)
  {
    RectPoints = stop_range_list[i];
    for (size_t j = 0; j < points_msg_now.points.size(); j++)
    {
      obstacle_point.x = points_msg_now.points[j].x;
      obstacle_point.y = points_msg_now.points[j].y;
      is_obstacle = pt_in_any_rect(obstacle_point, RectPoints[0], RectPoints[1],RectPoints[2],RectPoints[3]);
      if(is_obstacle)
      {
        if(stop_detecetion_number < stop_detecetion_threshold_) {
          stop_detecetion_number++;
          break;
        } else {
          publish_emergency_range(obstacle_point, RectPoints);
          obstacle_range.exist_obstacle = true;
          obstacle_range.type = SPEED_STOP;
          obstacle_range.range_index = i;
          obstacle_range.distance = dis_spacing_ * i;
          return;
        }
      }
    }
    if(is_obstacle) break;
  }

  if(!is_obstacle) stop_detecetion_number = 0;
  

  // 3. 遍历减速框，是否与点云有交集，有就减速
  for (size_t i = 0; i < deceleration_range_list.size(); i++)
  {
    RectPoints = deceleration_range_list[i];
    for (size_t j = 0; j < points_msg_now.points.size(); j++)
    {
      obstacle_point.x = points_msg_now.points[j].x;
      obstacle_point.y = points_msg_now.points[j].y;
      is_obstacle = pt_in_any_rect(obstacle_point, RectPoints[0], RectPoints[1],RectPoints[2],RectPoints[3]);
      if(is_obstacle)
      {
        if(deceleration_detecetion_number < deceleration_detecetion_threshold_) {
          deceleration_detecetion_number++;
          break;
        } else {
          publish_deceleration_range(obstacle_point, RectPoints); 
          obstacle_range.exist_obstacle = true;
          obstacle_range.type = DECELERATION;
          obstacle_range.range_index = i;
          obstacle_range.distance = dis_spacing_*i;
          return;
        }
      }
    }
    if(is_obstacle) break;
  }
  if(!is_obstacle) deceleration_detecetion_number = 0;

  obstacle_range.exist_obstacle = false;
}

void AEBNode::publish_params_status(){
  if (params_status_pub_->get_subscription_count()>0)
  {
    ubt_interface::msg::Status status_msg;
    status_msg.header.stamp = rclcpp::Clock().now();
    status_msg.enable_aeb = bool_enable_aeb;
    status_msg.stop_distance = stop_distance_;
    status_msg.speed_limit = speed_limit.speed_limit;
    status_msg.angular_limit = speed_limit.angular_limit;
    status_msg.emergency_type = holding_status.type;
    status_msg.emergency_type_str = aeb_type_str[holding_status.type];
    status_msg.current_obstacle_distance = holding_status.obstacle_distance;
    status_msg.footprint = {footprint[0].x,footprint[0].y,
                            footprint[1].x,footprint[1].y,
                            footprint[2].x,footprint[2].y,
                            footprint[3].x,footprint[3].y};
    status_msg.emergency_stop_footprint = {emergency_stop_footprint[0].x,emergency_stop_footprint[0].y,
                                           emergency_stop_footprint[1].x,emergency_stop_footprint[1].y,
                                           emergency_stop_footprint[2].x,emergency_stop_footprint[2].y,
                                           emergency_stop_footprint[3].x,emergency_stop_footprint[3].y};
    params_status_pub_->publish(status_msg);
  }
}

/**
 * @description: 发布触发避障的范围和障碍物   
 * @param:  obstacle 障碍物
 * @param:  RectPoints 触发避障的矩形框
 * @return: 
 */
void AEBNode::publish_emergency_range(geometry_msgs::msg::Point obstacle, std::vector<geometry_msgs::msg::Point> RectPoints)
{
  PointT point;
  point.x = obstacle.x;
  point.y = obstacle.y;

  publish_obstacle_marker(point, SPEED_STOP);
  publish_range_marker(RectPoints, SPEED_STOP);
}

/**
 * @description: 发布触发减速的范围和障碍物   
 * @param:  obstacle 障碍物
 * @param:  RectPoints 触发减速的矩形框
 * @return: 
 */
void AEBNode::publish_deceleration_range(geometry_msgs::msg::Point obstacle, std::vector<geometry_msgs::msg::Point> RectPoints)
{
  PointT point;
  point.x = obstacle.x;
  point.y = obstacle.y;
  publish_obstacle_marker(point,DECELERATION);
  publish_range_marker(RectPoints, DECELERATION);
}

void AEBNode::feedback_callback(const nav_msgs::msg::Odometry::SharedPtr odom)
{
  if (fabs(odom->twist.twist.linear.x) < feedback_valid_speed_limit)
  {
    odom->twist.twist.linear.x = 0.0;
  }

  if (fabs(odom->twist.twist.linear.y) < feedback_valid_speed_limit)
  {
    odom->twist.twist.linear.y = 0.0;
  }

  current_feedback = odom->twist.twist;

  if (!bool_enable_aeb)
  {
    
    return;
  }
  
  geometry_msgs::msg::Twist twist = odom->twist.twist;
  // if ( holding_status.is_holding && (holding_status.type == aeb_type::DECELERATION || holding_status.type == aeb_type::SPEED_STOP))
  if ( holding_status.is_holding)
  {
    double tmp_v = sqrt(stop_distance_ * 2.0 * deceleration_);
    // std::cout<< "stop_distance_" << stop_distance_ << "tmp_v" << tmp_v << std::endl;
    if (org_control.linear.x > feedback_valid_speed_limit && twist.linear.x < tmp_v)
    {
      twist.linear.x = tmp_v;
    }else if (org_control.linear.x < -feedback_valid_speed_limit && twist.linear.x > -tmp_v)
    {
      twist.linear.x = -tmp_v;
    }

    if (org_control.linear.y > feedback_valid_speed_limit && twist.linear.y < tmp_v)
    {
      twist.linear.y = tmp_v;
    }else if (org_control.linear.y < -feedback_valid_speed_limit && twist.linear.y > -tmp_v)
    {
      twist.linear.y = -tmp_v;
    }

    double feedback_speed = cal_linear_speed(twist.linear.x, twist.linear.y);
    if(feedback_speed == 0){  //原地自转
      double tmp_rot = sqrt(stop_angular_ * 2.0 * angle_deceleration_);
      if (org_control.angular.z > feedback_valid_angular_limit && twist.angular.z < tmp_rot)
      {
        twist.angular.z = tmp_rot;
      }else if (org_control.angular.z < -feedback_valid_angular_limit && twist.angular.z > -tmp_rot)
      {
        twist.angular.z = -tmp_rot;
      }
    }

    // RCLCPP_WARN(node->get_logger(),"holding");
    // printf("org x %.3f y %.3f z %.3f\n", org_control.linear.x, org_control.linear.y, org_control.angular.z);
    // printf("current_feedback x %.3f y %.3f z %.3f\n", current_feedback.linear.x, current_feedback.linear.y, current_feedback.angular.z);
    // printf("after x %.3f y %.3f z %.3f\n\n", twist.linear.x, twist.linear.y, twist.angular.z);
  }

  double linear_speed =  cal_linear_speed(twist.linear.x, twist.linear.y);
  
  double predict_path_len = linear_speed * linear_speed / (deceleration_ * 2.0);

  if (linear_speed < feedback_valid_speed_limit && fabs(twist.angular.z) < straight_angle_limit)
  {
    stop_range_list.clear();
    deceleration_range_list.clear();
    return;
  }

  double delta_t;
  double num;
  double pred_num;
  if (linear_speed >  feedback_valid_speed_limit)
  {
    delta_t = dis_spacing_ / linear_speed;
    num = predict_path_len / dis_spacing_;

    pred_num = std::max(2.0 / dis_spacing_ -  num , num);
  }else{
    double predict_path_angle = twist.angular.z * twist.angular.z / (angle_deceleration_ * 2.0);
    delta_t = angle_spacing_ / fabs(twist.angular.z);
    num = predict_path_angle / angle_spacing_;
    pred_num = num;
  }
  geometry_msgs::msg::PoseArray pose_array;
  geometry_msgs::msg::Pose pose;
  pose.orientation.w = 1.0;
  double pos_th = 0;
  pose_array.poses.push_back(pose);
  double last_delta_t_gain = num;
  for (size_t i = 0; i < int(num); i++)
  {
    double cos_th = cos(pos_th);
    double sin_th = sin(pos_th);
    double delta_x = (twist.linear.x * cos_th - twist.linear.y * sin_th) * delta_t;
    double delta_y = (twist.linear.x * sin_th + twist.linear.y * cos_th) * delta_t;
    double delta_th = twist.angular.z * delta_t;
    pose.position.x += delta_x;
    pose.position.y += delta_y;
    pos_th += delta_th;
    tf2::Quaternion myQuaternion;
    myQuaternion.setRPY( 0, 0, pos_th );
    tf2::convert(myQuaternion, pose.orientation);
    pose_array.poses.push_back(pose);
    last_delta_t_gain--;
  }
  if (last_delta_t_gain > 0)
  {
    double cos_th = cos(pos_th);
    double sin_th = sin(pos_th);
    double delta_x = (twist.linear.x * cos_th - twist.linear.y * sin_th) * delta_t * last_delta_t_gain;
    double delta_y = (twist.linear.x * sin_th + twist.linear.y * cos_th) * delta_t * last_delta_t_gain;
    double delta_th = twist.angular.z * delta_t;
    pose.position.x += delta_x;
    pose.position.y += delta_y;
    pos_th += delta_th;
    tf2::Quaternion myQuaternion;
    myQuaternion.setRPY( 0, 0, pos_th );
    tf2::convert(myQuaternion, pose.orientation);
    pose_array.poses.push_back(pose);
  }

  // publish_predict_path(pose_array);
  publish_predict_stop_range(pose_array);

  //减速范围
  for (size_t i = 0; i < pred_num; i++)
  {
    double cos_th = cos(pos_th);
    double sin_th = sin(pos_th);
    double delta_x = (twist.linear.x * cos_th - twist.linear.y * sin_th) * delta_t;
    double delta_y = (twist.linear.x * sin_th + twist.linear.y * cos_th) * delta_t;
    double delta_th = twist.angular.z * delta_t;
    pose.position.x += delta_x;
    pose.position.y += delta_y;
    pos_th += delta_th;
    tf2::Quaternion myQuaternion;
    myQuaternion.setRPY( 0, 0, pos_th );
    tf2::convert(myQuaternion, pose.orientation);
    pose_array.poses.push_back(pose);
  }

  publish_predict_path(pose_array);
  publish_predict_deceleration_range(pose_array);
}

/**
 * @description: 发布预测路径
 * @param: pose_array 位姿数组  
 */
void AEBNode::publish_predict_path(geometry_msgs::msg::PoseArray pose_array)
{
  visualization_msgs::msg::Marker line_strip;
  line_strip.header.frame_id = base_link_frame_;
  line_strip.header.stamp = rclcpp::Clock().now();
  line_strip.ns = "predict";
  line_strip.action = visualization_msgs::msg::Marker::ADD;
  line_strip.type = visualization_msgs::msg::Marker::LINE_STRIP;
  line_strip.pose.orientation.w = 1.0;
  line_strip.scale.x = 0.01;
  line_strip.color.r = 1.0;
  line_strip.color.g = 1.0;
  line_strip.color.b = 1.0;
  line_strip.color.a = 1.0;
  geometry_msgs::msg::Point p;
  for (size_t i = 0; i < pose_array.poses.size(); i++)
  {
    p = pose_array.poses[i].position;
    line_strip.points.push_back(p);
  }
  predict_path_pub_->publish(line_strip);
}

/**
 * @description:  计算并发布预测矩形框
 * @param:  
 * @return:   
 */
void AEBNode::publish_predict_stop_range(geometry_msgs::msg::PoseArray pose_array)
{
  visualization_msgs::msg::MarkerArray range_array;
  visualization_msgs::msg::Marker range;
  range.header.frame_id = base_link_frame_;
  range.header.stamp = rclcpp::Clock().now();
  range.ns = "range";
  range.lifetime = rclcpp::Duration(std::chrono::microseconds(100));
  range.action = visualization_msgs::msg::Marker::ADD;
  range.type = visualization_msgs::msg::Marker::LINE_STRIP;
  range.pose.orientation.w = 1.0;
  range.scale.x = 0.01;
  range.color = get_color(aeb_type::SPEED_STOP);
  stop_range_list.clear();
  for (size_t i = 0; i < pose_array.poses.size(); i++)
  {
    range.id = i;
    std::vector<geometry_msgs::msg::Point> RectPoints(4);
    RectPoints = get_rect(pose_array.poses[i]);
    stop_range_list.push_back(RectPoints);
    for (size_t j = 0; j < 4; j++)
    {
      range.points.push_back(RectPoints[j]);
    }
    range.points.push_back(RectPoints[0]);
    range_array.markers.push_back(range);
  }
  predict_stop_range_pub_->publish(range_array);
}

/**
 * @description:  计算并发布预测矩形框
 * @param:  
 * @return:   
 */
void AEBNode::publish_predict_deceleration_range(geometry_msgs::msg::PoseArray pose_array)
{
  visualization_msgs::msg::MarkerArray range_array;
  visualization_msgs::msg::Marker range;
  range.header.frame_id = base_link_frame_;
  range.header.stamp = rclcpp::Clock().now();
  range.ns = "deceleration_range";
  range.lifetime = rclcpp::Duration(std::chrono::microseconds(100));
  range.action = visualization_msgs::msg::Marker::ADD;
  range.type = visualization_msgs::msg::Marker::LINE_STRIP;
  range.pose.orientation.w = 1.0;
  range.scale.x = 0.01;
  range.color = get_color(aeb_type::DECELERATION);
  deceleration_range_list.clear();
  for (size_t i = 0; i < pose_array.poses.size(); i++)
  {
    range.id = i;
    // geometry_msgs::msg::Point *RectPoints = new geometry_msgs::msg::Point[4];
    std::vector<geometry_msgs::msg::Point> RectPoints(4);
    RectPoints = get_rect(pose_array.poses[i], expansion_range_);
    deceleration_range_list.push_back(RectPoints);
    for (size_t j = 0; j < 4; j++)
    {
      range.points.push_back(RectPoints[j]);
    }
    range.points.push_back(RectPoints[0]);
    range_array.markers.push_back(range);
  }
  predict_deceleration_range_pub_->publish(range_array);
}

/**
 * @description: 根据pose获取包围矩形
 * @param:  pose
 * @return: RectPoints
 */
std::vector<geometry_msgs::msg::Point> AEBNode::get_rect(geometry_msgs::msg::Pose pose, double expansion) 
{
  // RCLCPP_INFO(node->get_logger(),"get_rect [%.2f, %.2f] ", pose.position.x, pose.position.y);
  std::vector<geometry_msgs::msg::Point> RectPoints(4);
  double yaw=tf2::getYaw(pose.orientation);
  Eigen::Affine3f transform = Eigen::Affine3f::Identity();
  transform.translation() << pose.position.x, pose.position.y, 0.0;
  transform.rotate (Eigen::AngleAxisf (yaw, Eigen::Vector3f::UnitZ()));
  
  Eigen::Vector3f v3f;
  Eigen::Vector3f after;
  double footprint_x;
  double footprint_y;
  for (size_t i = 0; i < footprint.size(); i++)
  { 
    footprint_x = footprint[i].x > 0 ? footprint[i].x + expansion : footprint[i].x - expansion;
    footprint_y = footprint[i].y > 0 ? footprint[i].y + expansion : footprint[i].y - expansion;
    v3f << footprint_x, footprint_y, 0.0;
    after = transform*v3f;
    RectPoints[i].x = after[0];
    RectPoints[i].y = after[1];
  }
  return RectPoints;
}

/**
 * @description: 判断指定点是否在四边形内   
 * @param:  
 * @return:   
 */
bool AEBNode::pt_in_any_rect(geometry_msgs::msg::Point pCur, geometry_msgs::msg::Point pLT, geometry_msgs::msg::Point pLB, geometry_msgs::msg::Point pRB,
                                  geometry_msgs::msg::Point pRT) {
  //任意四边形有4个顶点
  int nCount = 4;
  geometry_msgs::msg::Point RectPoints[4] = {pLT, pLB, pRB, pRT};
  int nCross = 0;
  for (int i = 0; i < nCount; i++) {
      //依次取相邻的两个点
      geometry_msgs::msg::Point pStart = RectPoints[i];
      geometry_msgs::msg::Point pEnd = RectPoints[(i + 1) % nCount];

      //相邻的两个点是平行于x轴的，肯定不相交，忽略
      if (pStart.y == pEnd.y)
          continue;
      //交点在pStart,pEnd的延长线上，pCur肯定不会与pStart.pEnd相交，忽略
      if (pCur.y < std::min(pStart.y, pEnd.y) || pCur.y > std::max(pStart.y, pEnd.y))
          continue;
      //求当前点和x轴的平行线与pStart,pEnd直线的交点的x坐标
      double x =
              (double) (pCur.y - pStart.y) * (double) (pEnd.x - pStart.x) / (double) (pEnd.y - pStart.y) + pStart.x;
      //若x坐标大于当前点的坐标，则有交点
      if (x > pCur.x)
          nCross++;
  }
  // 单边交点为偶数，点在多边形之外
  return (nCross % 2 == 1);
}

bool AEBNode::get_footprint_from_param(){
  std::cout << "get_footprint_from_param" << std::endl;
  // XmlRpc::XmlRpcValue footprint_params;
  xmlrpc_env env;
  xmlrpc_value* footprint_params;
  std::string footprint_params_str = node->get_parameter("footprint").as_string();
  footprint_params = xmlrpc_string_new(&env, footprint_params_str.c_str());
  int size = xmlrpc_array_size(&env,footprint_params);
  xmlrpc_double num0;
  xmlrpc_double num1;
  xmlrpc_value *value;
  xmlrpc_value *num0_value;
  xmlrpc_value *num1_value;
  pcl::PointXY pt;
  if (!env.fault_occurred) {
      if (size != 4)
      {
        return false;
      }

      for (size_t i = 0; i < size; ++i) {
          xmlrpc_array_read_item(&env,footprint_params,i,&value);
          xmlrpc_array_read_item(&env,value,0,&num0_value);
          xmlrpc_array_read_item(&env,value,1,&num1_value);
          xmlrpc_read_double(&env,num0_value,&num0);
          xmlrpc_read_double(&env,num1_value,&num1);
          const double &x = num0;
          const double &y = num1;
          pt.x = x;
          pt.y = y;
          footprint.push_back(pt);
          RCLCPP_INFO_STREAM(node->get_logger(),"footprint " << i << " [" << x << " "<< y << "]");
      }
      first_footprint = footprint;
      xmlrpc_env_clean(&env);
      return true;
  }
  return false;
}

bool AEBNode::get_emergency_stop_footprint_from_param(){
  std::cout << "get_footprint_from_param" << std::endl;
  // XmlRpc::XmlRpcValue params;
  xmlrpc_env env;
  xmlrpc_value* params;
  std::string params_str = node->get_parameter("emergency_stop_footprint").as_string();
  params = xmlrpc_string_new(&env, params_str.c_str());
  int size = xmlrpc_array_size(&env,params);
  xmlrpc_double num0;
  xmlrpc_double num1;
  xmlrpc_value *value;
  xmlrpc_value *num0_value;
  xmlrpc_value *num1_value;
  pcl::PointXY pt;
  if (!env.fault_occurred) {
      if (size != 4)
      {
        return false;
      }

      for (size_t i = 0; i < size; ++i) {
          xmlrpc_array_read_item(&env,params,i,&value);
          xmlrpc_array_read_item(&env,value,0,&num0_value);
          xmlrpc_array_read_item(&env,value,1,&num1_value);
          xmlrpc_read_double(&env,num0_value,&num0);
          xmlrpc_read_double(&env,num1_value,&num1);
          const double &x = num0;
          const double &y = num1;
          pt.x = x;
          pt.y = y;
          emergency_stop_footprint.push_back(pt);
          RCLCPP_INFO_STREAM(node->get_logger(),"emergency_stop_footprint " << i << " [" << x << " "<< y << "]");
      }
      xmlrpc_env_clean(&env);
      return true;
  }
  return false;
}

bool AEBNode::get_speed_limit_param(){
  std::cout << "get_speed_limit_param" << std::endl;
  // XmlRpc::XmlRpcValue distance_params;
  xmlrpc_env env;
  xmlrpc_value* distance_params;
  std::string distance_params_str = node->get_parameter("aeb_obstacle_distance").as_string();
  distance_params = xmlrpc_string_new(&env, distance_params_str.c_str());
  int size = xmlrpc_array_size(&env,distance_params);
  xmlrpc_double num;
  xmlrpc_value *value;
  if (!env.fault_occurred) {
      for (size_t i = 0; i < size; ++i) {
        
        xmlrpc_array_read_item(&env,distance_params,i,&value);
        xmlrpc_read_double(&env,value,&num);
        const double &x = num;
        aeb_obstacle_distance.push_back(x);
      }
  }
  

  // XmlRpc::XmlRpcValue speed_params;
  xmlrpc_value* speed_params;
  std::string speed_params_str = node->get_parameter("aeb_obstacle_speed").as_string();
  speed_params = xmlrpc_string_new(&env, speed_params_str.c_str());
  size = xmlrpc_array_size(&env,speed_params);

  if (!env.fault_occurred) {
      for (size_t i = 0; i < size; ++i) {
        xmlrpc_array_read_item(&env,speed_params,i,&value);
        xmlrpc_read_double(&env,value,&num);
        const double &x = num;
        aeb_obstacle_speed.push_back(x);
      }
  }
  xmlrpc_env_clean(&env);

  if (aeb_obstacle_distance.size() < 1)
  {
    RCLCPP_ERROR(node->get_logger(),"aeb_obstacle_distance not configured !");
    return false;
  }

  if (aeb_obstacle_distance.size() != aeb_obstacle_speed.size())
  {
    RCLCPP_ERROR(node->get_logger(),"aeb_obstacle_distance and aeb_obstacle_speed not match !");
    return false;
  }

  //检查是否符合要求
  //1. 距离是否递增
  for(int i=1;i<aeb_obstacle_distance.size();i++)
  {
    if(aeb_obstacle_distance[i]<=aeb_obstacle_distance[i-1]){
      RCLCPP_WARN(node->get_logger(),"aeb_obstacle_distance has to be increasing !");
      return false;
    }
  }

  double tmp_v = 0.0;
  bool check_result = true;
  std::vector<double> speed_tmp;
  //2. 检查速度设置是否合理
  for (int i = 0; i < aeb_obstacle_distance.size(); i++)
  {
    if (0 == i)
    {
      tmp_v =  sqrt(aeb_obstacle_distance[i] * 2.0 * deceleration_);
      if (aeb_obstacle_speed[i] >= tmp_v)
      {
        check_result = false;
        speed_tmp.push_back(std::min(tmp_v * 0.9, 0.1));
      }else{
        speed_tmp.push_back(aeb_obstacle_speed[i]);
      }
    }else{
      tmp_v =  sqrt(aeb_obstacle_distance[i-1] * 2.0 * deceleration_);
      if (aeb_obstacle_speed[i] >= tmp_v)
      {
        check_result = false;
        speed_tmp.push_back(tmp_v * 0.9);
      }else{
        speed_tmp.push_back(aeb_obstacle_speed[i]);
      } 
    }
  }

  if (!check_result)
  {
    for (int i = 0; i < speed_tmp.size(); i++){
      RCLCPP_ERROR(node->get_logger(),"Suggested speed %.3f <= %.3f", aeb_obstacle_distance[i], speed_tmp[i]);
    }
  }
  return check_result;
}

/**
 * @description:  根据距离获得速度限制
 * @param: 
 * @return: 速度   
 */
double AEBNode::get_speed_limit(ObstacleRange obstacle_range){
  double speed = 999;
  for (int i = aeb_obstacle_distance.size() -1; i >= 0 ; i--)
  {
    // printf("%d distance:%.3f level:%.3f speed:%.3f \n",i, obstacle_range.distance, aeb_obstacle_distance[i], aeb_obstacle_speed[i]);
    if (obstacle_range.distance <= aeb_obstacle_distance[i])
    {
      speed = aeb_obstacle_speed[i];
    }else{
      // printf("break distance %.3f limit_dis %.3f\n", obstacle_range.distance, aeb_obstacle_distance[i]);
      break;
    }
  }
  // printf("speed:%.3f \n\n", speed);
  return speed;   
}

/**
 * @description: 检查是否需要状态保持   
 * @param:  
 * @return:   
 */
bool AEBNode::check_need_holding(double speed)
{
  if (!holding_status.is_holding)
  {
    return false;
  }

  if ((rclcpp::Clock().now() - holding_status.start_time).seconds() > holding_time_) //超时恢复
  {
    return false;
  }

  if ( holding_status.linear_speed >= fabs(speed)) //避障速度比目标速度高
  {
    return false;
  }
  
  return true;  
}

/**
 * @description: 根据限速改变twist
 * @param:   
 * @return: 改变后的控制
 */
geometry_msgs::msg::Twist AEBNode::cal_twist(geometry_msgs::msg::Twist org, double limit_speed, bool need_holding)
{
  double org_speed = cal_linear_speed(org.linear.x, org.linear.y);
  double speed = org_speed;
  double angular = org.angular.z;
  if (speed != 0)
  {
    speed = std::min(org_speed, limit_speed);

    if (org.angular.z != 0)
    {
      double radius =  fabs(org_speed / org.angular.z);
      double angle_speed = fabs(speed / radius);
      angular = org.angular.z > 0 ? angle_speed : -angle_speed;
    }
  }else{
    if (org.angular.z != 0)
    {
      angular = angular > 0 ? std::min(angular, aeb_obstacle_angular_speed_) : std::max(angular, -aeb_obstacle_angular_speed_);
      if(need_holding){
        angular = holding_status.twist.angular.z;
      }
    }
  }
  double rate = 1.0;
  if ( org_speed > 0 ) rate = speed / org_speed;

  org.linear.x *= rate;
  org.linear.y *= rate;
  org.angular.z = angular;
  
  return org;
}


/**
 * @description: 根据限速和角度限制改变twist,有冲突时限制更小的那个生效,保证轨迹不变
 * @param:   
 * @return: 改变后的控制
 */
geometry_msgs::msg::Twist AEBNode::cal_twist_by_limits(geometry_msgs::msg::Twist org, SpeedLimit limits)
{
  if (limits.speed_limit > 100.0 && limits.angular_limit > 100.0 )
  {
    return org;
  }
  
  double org_speed = cal_linear_speed(org.linear.x, org.linear.y);

  double speed = org_speed;
  double angular = org.angular.z;

  if (org_speed != 0) //速度不为0
  {
    speed = std::min(org_speed, limits.speed_limit);

    if (org.angular.z != 0) //非直线运动
    {
      double radius =  fabs(org_speed / org.angular.z);
      double angle_speed = fabs(speed / radius);
      // printf("org x %.3f  z %.3f \n", org.linear.x, org.angular.z);
      // printf("speed limit %.3f , angle limit tmp %.3f limits.angular_limit %.3f\n", speed, angle_speed, limits.angular_limit);

      if (angle_speed > limits.angular_limit) //按速度限制之后角速度超了
      {
        angle_speed = limits.angular_limit;
        speed = angle_speed * radius;
      }
      angular = org.angular.z > 0 ? angle_speed : -angle_speed;
      // printf("final speed %.3f angle %.3f\n", speed, angle_speed);
    }
  }else{ //原地旋转
    if (org.angular.z != 0)
    {
      angular = angular > 0 ? std::min(angular, limits.angular_limit) : std::max(angular, -limits.angular_limit);
    }
  }
  
  double rate = 1.0;
  if ( org_speed > 0 ) rate = speed / org_speed;

  org.linear.x *= rate;
  org.linear.y *= rate;
  org.angular.z = angular; 
  return org;
}


/**
 * @description: 速度平滑,根据线加速度限制车体加减速
 * @param:   
 * @return: 改变后的控制
 */
geometry_msgs::msg::Twist AEBNode::speed_smooth(geometry_msgs::msg::Twist org)
{ 
  double delta_t = std::min((rclcpp::Clock().now() - last_smooth_time).seconds(), 0.1);  //控制不该低于10hz
  // last_smooth_time = rclcpp::Clock().now();
  if (delay_time_ > 0)
  {
    delta_t += delay_time_;
  }
  
  double org_speed = cal_linear_speed(org.linear.x, org.linear.y);
  double speed = org_speed;
  double angular = org.angular.z;
  //@todo特别小的数据暂未处理
  // RCLCPP_INFO(node->get_logger(),"============================== speed_smooth ======================================");
  // RCLCPP_INFO(node->get_logger(),"org speed angular (%.3f, %.3f)", speed, angular);
  //变换方向不做处理,直接转发
  if (org.linear.x * current_feedback.linear.x < 0 || org.linear.y * current_feedback.linear.y < 0)
  {
    return org;
  }

  double feed_back_speed = cal_linear_speed(current_feedback.linear.x , current_feedback.linear.y);
  double limit_speed = 0.0;
  if (org_speed != 0) //速度不为0
  {
    //加速
    if (org_speed > feed_back_speed)
    {
      limit_speed = feed_back_speed + acceleration_ * delta_t;
      speed = std::min(org_speed, limit_speed);
      // RCLCPP_INFO(node->get_logger(),"speed1 org %.3f feed %.3f limit %.3f)", org_speed, feed_back_speed, limit_speed);
    }else{
      limit_speed = feed_back_speed - deceleration_ * delta_t;
      speed = std::max(org_speed, limit_speed);
      // RCLCPP_INFO(node->get_logger(),"speed2 org %.3f feed %.3f limit %.3f)", org_speed, feed_back_speed, limit_speed);
    }

    if (org.angular.z != 0) //非直线运动
    {
      double radius =  fabs(org_speed / org.angular.z);
      double angle_speed = fabs(speed / radius);
      angular = org.angular.z > 0 ? angle_speed : -angle_speed;
    }
    // RCLCPP_INFO(node->get_logger(),"after1 speed angular (%.3f, %.3f)", speed, angular);

  }else{ //原地旋转
    if (org.angular.z * current_feedback.angular.z >= 0)
    {
      if (org.angular.z !=0 )
      {
        double rotation_limit = 0.0;
        double rotation_org = fabs(org.angular.z);
        double rostation_current = fabs(current_feedback.angular.z);
        if (rotation_org > rostation_current) //加速
        {
          rotation_limit = std::min(rotation_org, rostation_current + angle_deceleration_ * delta_t);
          // RCLCPP_INFO(node->get_logger(),"rotation1 org %.3f feed %.3f limit %.3f)", rotation_org, rostation_current, rotation_limit);
        }else{
          rotation_limit = std::max(rotation_org, rostation_current - angle_deceleration_ * delta_t);
          // RCLCPP_INFO(node->get_logger(),"rotation2 org %.3f feed %.3f limit %.3f)", rotation_org, rostation_current, rotation_limit);
        }
        angular *= rotation_limit / rotation_org;
      }
      //下发为0不做处理
    }
    // RCLCPP_INFO(node->get_logger(),"after2 speed angular (%.3f, %.3f)", speed, angular);
    //变换方向不做处理直接转发
  }
  
  double rate = 1.0;
  if ( org_speed > 0 ) rate = speed / org_speed;

  org.linear.x *= rate;
  org.linear.y *= rate;
  org.angular.z = angular; 
  return org;
}


bool AEBNode::check_emergency_stop()
{
  pcl::PointCloud<PointT>::Ptr cloud_final{ new pcl::PointCloud<PointT> };//加工后的pcl格式
  pcl::CropBox<PointT> clipper;//要包含的头文件，在官方文档里
  Eigen::Vector4f min_point;
  Eigen::Vector4f max_point;
  min_point << emergency_stop_footprint[2].x, emergency_stop_footprint[2].y, -1.0, 1.0;//cube的两个对角点
  max_point << emergency_stop_footprint[0].x, emergency_stop_footprint[0].y, 1.0, 1.0;
  clipper.setMin(min_point);
  clipper.setMax(max_point);
  clipper.setInputCloud(points_msg_now.makeShared());
  clipper.filter(*cloud_final);
  clipper.setNegative(false);//默认为false
  if (cloud_final->points.size() > 0)
  {
    //1. 停车
    RCLCPP_WARN(node->get_logger(),"[AEB] emergency stop !");
    std_msgs::msg::Bool result;
    result.data = true;
    emergency_stop_pub_->publish(result);

    //2. 发布范围框
    // geometry_msgs::msg::Point *RectPoints = new geometry_msgs::msg::Point[4];
    std::vector<geometry_msgs::msg::Point> RectPoints(4);
    geometry_msgs::msg::Point point;
    for (size_t i = 0; i < emergency_stop_footprint.size(); i++)
    {
      point.x = emergency_stop_footprint[i].x;
      point.y = emergency_stop_footprint[i].y;
      RectPoints[i] = point;
    }   
    publish_range_marker(RectPoints, EMERGENCY_STOP);
    publish_obstacle_marker(cloud_final->points[0],EMERGENCY_STOP);
    return true;
  }
  return false;
}

/**
 * @description: 发布检测框
 * @param:  
 * @return:   
 */
void AEBNode::publish_range_marker(std::vector<geometry_msgs::msg::Point> range, aeb_type type){
  visualization_msgs::msg::Marker range_marker;
  range_marker.header.frame_id = base_link_frame_;
  range_marker.header.stamp = rclcpp::Clock().now();
  range_marker.ns = "obstacle_range";
  range_marker.lifetime = rclcpp::Duration(std::chrono::seconds(5));
  range_marker.action = visualization_msgs::msg::Marker::ADD;
  range_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
  range_marker.pose.orientation.w = 1.0;
  range_marker.scale.x = 0.05;  
  range_marker.color = get_color(type);
  for (int i = 0; i < 4; i++)
  {
    range_marker.points.push_back(range[i]);
  }
  range_marker.points.push_back(range[0]);
  obstacle_range_pub_->publish(range_marker);
}

/**
 * @description: 发布障碍物 
 * @param:  
 * @return:   
 */
void AEBNode::publish_obstacle_marker(PointT point, aeb_type type){
  visualization_msgs::msg::Marker obstacle_marker;
  obstacle_marker.header.frame_id = base_link_frame_;
  obstacle_marker.header.stamp = rclcpp::Clock().now();
  obstacle_marker.ns = "obstacle";
  obstacle_marker.lifetime = rclcpp::Duration(std::chrono::seconds(5));
  obstacle_marker.action = visualization_msgs::msg::Marker::ADD;
  obstacle_marker.type = visualization_msgs::msg::Marker::SPHERE;
  obstacle_marker.pose.position.x = point.x;
  obstacle_marker.pose.position.y = point.y;
  obstacle_marker.pose.orientation.w = 1.0;
  obstacle_marker.scale.x = 0.15;
  obstacle_marker.scale.y = 0.15;
  obstacle_marker.scale.z = 0.15;
  obstacle_marker.color = get_color(type);
  obstacles_pub_->publish(obstacle_marker);
}

/**
 * @description: 获取标志的颜色
 * @param:  
 * @return:   
 */
std_msgs::msg::ColorRGBA AEBNode::get_color(aeb_type type){
  std_msgs::msg::ColorRGBA color;
  switch (type) {
    case EMERGENCY_STOP:
      color.r = 1;   color.g = 0;   color.b = 0;   color.a=1.0;//红色
      break;
    case SPEED_STOP:
      color.r = 1;   color.g = 1;   color.b = 0;   color.a=1.0;//黄色
      break;
    case DECELERATION:
      color.r = 0;   color.g = 0;   color.b = 1;   color.a=1.0;//蓝色
      break;
  }
  return color;
}

void AEBNode::publish_status(aeb_type type){
  std_msgs::msg::String status;
  switch(type) {
    case aeb_type::OTHER: 
      status.data = "ao_vel_normal"; 
      break;
    case aeb_type::DECELERATION:   
      status.data = "ao_vel_slow";   
      break;
    case aeb_type::SPEED_STOP:   
      status.data = "ao_vel_stop";   
      break;
    case aeb_type::EMERGENCY_STOP:   
      status.data = "ao_vel_stop";   
      break;
    default: 
      status.data = "ao_vel_normal";
  }
  status_pub_->publish(status);

  std_msgs::msg::Int8 obs_status;
  switch(type) {
    case aeb_type::OTHER: 
      obs_status.data = 0; 
      break;
    case aeb_type::DECELERATION:   
      obs_status.data = 2;   
      break;
    case aeb_type::SPEED_STOP:   
      obs_status.data = 1;   
      break;
    case aeb_type::EMERGENCY_STOP:   
      obs_status.data = 1;   
      break;
    default: 
      obs_status.data = 0;
  }
  obs_status_pub_->publish(obs_status);
}

void AEBNode::set_holding_status(bool is_holding, aeb_type type, geometry_msgs::msg::Twist twist, double linear_speed, double obstacle_distance){
  holding_status.is_holding = is_holding;
  holding_status.type = type;
  holding_status.start_time = rclcpp::Clock().now();
  holding_status.twist = twist;
  holding_status.linear_speed = linear_speed;
  holding_status.obstacle_distance = obstacle_distance;
}

double AEBNode::cal_linear_speed(double x, double y){
  double speed;
  speed = std::sqrt( x*x + y*y );
  return speed;
}


int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  node =  std::make_shared<rclcpp::Node>("aeb_node");
  RCLCPP_INFO_STREAM(node->get_logger(),"Start AEB Node !");
  AEBNode aeb_node;
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}

